//file:system/lpc/msgqueue.c
//autor:jiangxinpeng
//time:2021.5.13
//copyright:(C) by jiangxinpeng,All right are reserved.

#include <os/msgqueue.h>
#include <os/debug.h>
#include <os/schedule.h>
#include <sys/ioctl.h>
#include <os/fifo.h>
#include <os/safety.h>
#include <sys/lpc.h>
#include <sys/ipc.h>
#include <lib/stdio.h>
#include <lib/errno.h>

static msgqueue_t *msgqueue_table;
DEFINE_SEMAPHORE(msgqueue_mutex, 1);

msgqueue_t *MsgQueueAlloc(char *name)
{
    msgqueue_t *msgqueue;

    for (int i = 0; i < MSGQUEUE_MAX_NUM; i++)
    {
        msgqueue = msgqueue_table + i;
        if (msgqueue->name[0] == '\0')
        {
            memcpy(msgqueue->name, name, MSGQUEUE_NAME_LEN);
            msgqueue->name[MSGQUEUE_NAME_LEN - 1] = '\0';
            list_init(&msgqueue->list);
            msgqueue->msgnum = 0;
            msgqueue->msgsize = 0;
            WaitQueueInit(&msgqueue->sender);
            WaitQueueInit(&msgqueue->recver);
            return msgqueue;
        }
    }
    return NULL;
}

void MsgQueueFree(msgqueue_t *msgqueue)
{
    memset(msgqueue->name, 0, MSGQUEUE_NAME_LEN);
}

static msgqueue_t *MsgQueueFindByName(char *name)
{
    msgqueue_t *msgqueue;
    for (int i = 0; i < MSGQUEUE_MAX_NUM; i++)
    {
        msgqueue = msgqueue_table + i;
        if (msgqueue->name[0])
        {
            if (!strcmp(msgqueue->name, name))
                return msgqueue;
        }
    }
    return NULL;
}

static msgqueue_t *MsgQueueFindById(int msgqueueid)
{
    msgqueue_t *msgqueue;
    for (int i = 0; i < MSGQUEUE_MAX_NUM; i++)
    {
        msgqueue = msgqueue_table + i;
        if (msgqueue->name[0] && msgqueue->id == msgqueueid)
        {

            return msgqueue;
        }
    }
    return NULL;
}

int MsgQueueGet(char *name, uint32_t flags)
{
    int create_new;
    msgqueue_t *msgqueue;
    int ret = -1;

    if (!name)
        return -EINVAL;
    SemaphoreDown(&msgqueue_mutex);
    if (flags & IPC_CREATE)
    {
        if (flags & IPC_EXCL)
            create_new = 1;
        msgqueue = MsgQueueFindByName(name);
        if (msgqueue)
        {
            if (create_new)
                goto err;
            ret = msgqueue->id;
        }
        else
        {
            msgqueue = MsgQueueAlloc(name);
            if (!msgqueue)
                goto err;
            ret = msgqueue->id;
        }
    }
err:
    SemaphoreUp(&msgqueue_mutex);
    return ret;
}

int MsgQueuePut(int msgqueueid)
{
    msgqueue_t *msgqueue;

    SemaphoreDown(&msgqueue_mutex);
    msgqueue = MsgQueueFindById(msgqueueid);
    if (msgqueue)
    {
        MsgQueueFree(msgqueue);
        SemaphoreUp(&msgqueue_mutex);
        return 0;
    }
    SemaphoreUp(&msgqueue_mutex);
    return -1;
}

int MsgQueueSend(int msgqueueid, void *msgbuff, size_t size, int msgflags)
{
    msgqueue_t *msgqueue;
    msg_t *msg;
    int64_t *msg_header;

    SemaphoreDown(&msgqueue_mutex);
    msgqueue = MsgQueueFindById(msgqueueid);
    if (!msgqueue)
    {
        SemaphoreUp(&msgqueue_mutex);
        KPrint(PRINT_ERR "%s: not found message queue! id=%d\n", __func__, msgqueueid);
        return -1;
    }
    SemaphoreUp(&msgqueue_mutex);
    SemaphoreDown(&msgqueue->mutex);
    //msgsize no can above limit
    if (size > msgqueue->msgsize)
    {
        size = msgqueue->msgsize;
    }
    if (msgqueue->msgnum > MSGQUEUE_MAX_MSG)
    {
        if (msgflags & IPC_NOWAIT)
        {
            SemaphoreUp(&msgqueue->mutex);
            return -1;
        }
        KPrint("%s: cur task %s into blocked!\n", __func__, cur_task->name);
        WaitQueueAdd(&msgqueue->sender, cur_task);
        WaitQueueWakeup(&msgqueue->recver);
        SemaphoreUp(&msgqueue->mutex);
        TaskBlock(TASK_BLOCKED);
        KPrint("%s: task %s exit blocked!\n", __func__, cur_task->name);
        SemaphoreDown(&msgqueue_mutex); //restart down semaphore when task wakeup
    }
    msg = KMemAlloc(sizeof(msg_t));
    if (!msg)
    {
        SemaphoreUp(&msgqueue->mutex);
        return -1;
    }
    msg->buff = KMemAlloc(size);
    if (!msg->buff)
    {
        SemaphoreUp(&msgqueue->mutex);
        return -1;
    }
    msg_header = msgbuff;
    MsgInit(msg, *msg_header, msg_header + 1, size);
    list_add_after(&msg->list, &msgqueue->list);
    msgqueue->msgnum++;
    WaitQueueWakeup(&msgqueue->recver);
    SemaphoreUp(&msgqueue->mutex);
    return 0;
}

/**
 * @size: message size
 * @msgflags: message flags,IPC_NOWAIT is no wait when queue full
 *            msgtype>0 and msgflags assign IPC_EXCEPT,receive first type no equal msgtype message 
 * @msgtype: message type deal
 *              type=0,return queue first message
 *              type>0,return queue first fit type message
 *              type<0,return queue first fit type absvalue message    
 * @return: succeed return 0,or return -1
 */
int MsgQueueRecv(int msgqueueid, void *msgbuff, size_t size, int64_t msgtype, int64_t msgflags)
{
    msgqueue_t *msgqueue;
    msg_t *msg, *tmp;
    uint32_t len;
    int64_t *msgheader;

    SemaphoreDown(&msgqueue_mutex);
    msgqueue = MsgQueueFindById(msgqueueid);
    if (!msgqueue)
    {
        SemaphoreUp(&msgqueue_mutex);
        KPrint(PRINT_ERR "%s: no found message queue! id=%d\n", __func__, msgqueueid);
        return -1;
    }
    SemaphoreUp(&msgqueue_mutex);
    SemaphoreDown(&msgqueue->mutex);
    if (!msgqueue->msgnum)
    {
        if (msgflags & IPC_NOWAIT)
        {
            SemaphoreUp(&msgqueue->mutex);
            return -1;
        }
        KPrint(PRINT_WARNNING "%s: cur task:%s into blocked!\n", __func__, cur_task->name);
        WaitQueueAdd(&msgqueue->recver, cur_task);
        WaitQueueWakeup(&msgqueue->sender);
        SemaphoreUp(&msgqueue->mutex);
        TaskBlock(TASK_BLOCKED);
        KPrint(PRINT_INFO "%s: task：%s exit blocked!\n", __func__, cur_task->name);
        SemaphoreDown(&msgqueue->mutex);
    }
    if (msgtype > 0)
    {
        if (msgflags & IPC_EXCEPT)
        {
            list_traversal_all_owner_to_next(tmp, &msgqueue->list, list)
            {
                if (tmp->type != msgtype)
                {
                    msg = tmp;
                    break;
                }
            }
        }
        else
        {
            list_traversal_all_owner_to_next(tmp, &msgqueue->list, list)
            {
                if (tmp->type == msgtype)
                {
                    msg = tmp;
                    break;
                }
            }
        }
    }
    else
    {
        //msgtype is 0 and return first message
        if (msgtype == 0)
        {
            msg = list_first_owner(&msgqueue->list, msg_t, list);
        }
        else
        {
            //if msgtype is < 0 and do abs
            msgtype = ABS(msgtype);
            list_traversal_all_owner_to_next(tmp, &msgqueue->list, list)
            {
                if (tmp->type <= msgtype)
                {
                    msg = tmp;
                    break;
                }
            }
        }
    }
    if (!msg)
    {
        SemaphoreUp(&msgqueue->mutex);
        KPrint(PRINT_ERR "%s: message no fount!\n");
        return -1;
    }
    list_del(&msg->list); //remove msg from queue
    msgqueue->msgnum--;
    len = msg->length;
    if (msgflags & IPC_NOERROR)
    {
        if (len > size)
            len = size;
    }
    //copy to buffer
    msgheader = msgbuff;
    *msgheader = msg->type;
    memcpy(msgheader + 1, msg->buff, len);
    MemFree(msg);
    WaitQueueWakeup(&msgqueue->sender);
    SemaphoreUp(&msgqueue->mutex);
    return len;
}

void MsgInit(msg_t *msg, int64_t type, void *buff, size_t length)
{
    list_init(&msg->list);
    msg->type = type;
    msg->length = length;
    memcpy(msg->buff, buff, length);
}

void MsgQueueInit()
{
    int i;

    msgqueue_table = (msgqueue_t *)KMemAlloc(sizeof(msgqueue_t) * MSGQUEUE_MAX_NUM);
    if (!msgqueue_table)
        Panic(PRINT_WARNNING "%s:alloc memory for msgqueue failed!\n", __func__);
    for (i = 0; i < MSGQUEUE_MAX_NUM; i++)
    {
        msgqueue_table[i].id = i + 1;
        list_init(&msgqueue_table[i].list);
        msgqueue_table[i].msgnum = 0;
        msgqueue_table[i].msgsize = 0;
        SemaphoreInit(&msgqueue_table[i].mutex, 1);
        memset(msgqueue_table[i].name, 0, MSGQUEUE_NAME_LEN);
    }
    KPrint("[msgqueue] init msgqueue done.");
}

int SysMsgQueueSend(int msgqueueid, void *msgbuff, size_t size, int msgflags)
{

    if (!msgbuff)
        return -EINVAL;
    if (SafetyCheckRange(msgbuff, size) < 0)
        return -EINVAL;
    return MsgQueueSend(msgqueueid, msgbuff, size, msgflags);
}

int SysMsgQueueRecv(int msgqueueid, void *msgbuff, size_t size, int msgflags)
{
    int64_t *msgtype;

    if (!msgbuff)
        return -EINVAL;
    if (SafetyCheckRange(msgbuff, size) < 0)
        return -EINVAL;
    msgtype = msgbuff;
    return MsgQueueRecv(msgqueueid, msgbuff, size, *msgtype, msgflags);
}

int SysMsgQueueGet(char *name, uint32_t flags)
{
    if (!name)
        return -EINVAL;
    if (SafetyCheckRange(name, MSGQUEUE_NAME_LEN) < 0)
        return -EINVAL;
    return MsgQueueGet(name, flags);
}

int SysMsgQueuePut(int msgqueueid)
{
    return MsgQueuePut(msgqueueid);
}
